package chainSimplification;

import java.util.*;

public class Simplification {

	public static float distance(Vector3f v1, Vector3f v2, Vector3f n) {
		float d1 = v1.subtract(n).length();
		float d2 = v2.subtract(n).length();
		
		Vector3f va = v2.subtract(v1);
		Vector3f vb = n.subtract(v1);
		
		float d3 = va.cross(vb).length() / va.add(vb).length() / 2;

		if (d1 > d2)
			d1 = d2;
		Vector3f a = v2.subtract(v1);
		Vector3f b = n.subtract(v1);

		float shadow = a.normalize().dot(b);

		if (shadow < 0 || shadow > a.length()) {
			return d1;
		}
		return d3;
	}

	public static Path simplifyPath(Path original, float angle, float epsilon) {
		Vector<Vertex> path = original.getVertices();
		int best[][] = new int[original.getVertices().size()][original
				.getVertices().size()];
		int trackBack[][] = new int[path.size()][path.size()];
		for (int i = 0; i < best.length; i++) {
			for (int j = i + 1; j < best.length; j++) {
				float maxDist = 0;
				for (int k = i; k <= j; k++) {
					float dist = distance(path.get(i), path.get(j), path.get(k));
					if (dist > maxDist)
						maxDist = dist;
				}
				if (maxDist > epsilon) {
					best[i][j] = -1;
					trackBack[i][j] = -1;
					continue;
				}
				if (i == 0) {
					best[i][j] = 1;
					trackBack[i][j] = -1;
				} else {
					int min = -1;
					int track = -1;
					for (int k = 0; k < i; k++) {
						if (best[k][i] < 0)
							continue;
						Vector3f v1 = path.get(i).subtract(path.get(k));
						Vector3f v2 = path.get(j).subtract(path.get(i));
						if (v1.angleBetween(v2) < angle
								&& (track == -1 || min > best[k][i])) {
							track = k;
							min = best[k][i];
						}
					}
					if (track != -1) {
						best[i][j] = min + 1;
						trackBack[i][j] = track;
					} else {
						best[i][j] = -1;
						trackBack[i][j] = -1;
					}
				}
			}
		}
		Vector<Integer> simpV = new Vector<Integer>();

		int cur = path.size() - 1;
		int minSolution = -1;
		int last = -1;
		for (int i = 0; i < cur; i++) {
			if ((last == -1 || (minSolution > best[i][cur]))
					&& trackBack[i][cur] != -1) {
				last = i;
				minSolution = best[i][cur];
			}
		}
		if (last == -1)
			return null;
		while (last != -1) {
			simpV.add(cur);
			int next = trackBack[last][cur];
			cur = last;
			last=next;
		}
		Path sim = new Path();
		for (int i = simpV.size() - 1; i >= 0; i--) {
			sim.addVertex(path.get(simpV.get(i)));
		}
		return sim;
	}

	public static void main(String s[]) {
		Vector3f v1 = new Vector3f(1, 0, 0);
		Vector3f v2 = new Vector3f(2, 0, 0);
		Vector3f v3 = new Vector3f(3, 0, 0);
		Path path = new Path();
		path.addVertex(new Vertex(v1));
		path.addVertex(new Vertex(v2));
		path.addVertex(new Vertex(v3));

		Path p = simplifyPath(path, (float) Math.PI / 2, 0);
		if (p != null)
			System.out.println(p.vertices);
		else
			System.out.println("No solution");
	}
}
